﻿using System;

namespace VectorMath
{
    public class Pnt3D
    {
        public double x;
        public double y;
        public double z;

        public float xf { get { return (float)x; } }
        public float yf { get { return (float)y; } }
        public float zf { get { return (float)z; } }

        public double SquaredLength { get { return Math.Pow(x, 2) + Math.Pow(y, 2)+Math.Pow(z,2); } }

        public double Length { get { return Math.Sqrt(SquaredLength); } }

        public Pnt3D(double x, double y, double z)
        {
            this.x = x;
            this.y = y;
            this.z = z;
        }

        public override string ToString()
        {
            return "{" + x + ", " + y + ", " + z + "}";
        }

        public void Add(Pnt3D p)
        {
            x += p.x;
            y += p.y;
            z += p.z;
        }

        public void Sub(Pnt3D p)
        {
            x -= p.x;
            y -= p.y;
            z -= p.z;
        }

        public void Times(double d)
        {
            x *= d;
            y *= d;
            z *= d;
        }

        /// <summary>
        /// this = this + (p * d) .
        /// </summary>
        public void ScaleAdd(Pnt3D p, double d)
        {
            x += p.x * d;
            y += p.y * d;
            z += p.z * d;
        }

        /// <summary>
        /// Set this point's value.
        /// </summary>
        public void Set(double x, double y, double z)
        {
            this.x = x;
            this.y = y;
            this.z = z;
        }

        /// <summary>
        /// Normalize this vector.
        /// </summary>
        public void Normalize()
        {
            double length = Length; //need to save this since it will change
            x /= length;
            y /= length;
            z /= length;
        }

        /// <summary>
        /// Returns this dot p.
        /// </summary>
        public double Dot(Pnt3D p)
        {
            return x * p.x + y * p.y + z * p.z;
        }

        /// <summary>
        /// Returns (y*p.z - z *p.y, z*p.x - x*p.z, x*p.y - y *p.x).
        /// </summary>
        public Pnt3D Cross(Pnt3D p)
        {
            return new Pnt3D(y*p.z - z *p.y, z*p.x - x*p.z, x*p.y - y *p.x);
        }

        /// <summary>
        /// Returns a new point equal to a - b.
        /// </summary>
        public static Pnt3D Difference(Pnt3D a, Pnt3D b)
        {
            return new Pnt3D(a.x - b.x, a.y - b.y, a.z - b.z);
        }

        public static void Add(Pnt3D a, Pnt3D b, ref Pnt3D c){
            c.x = a.x + b.x;
            c.y = a.y + b.y;
            c.z = a.z + b.z;
        }


        public static void Add(Pnt3D a, Pnt3D b, Pnt3D c, ref Pnt3D d)
        {
            d.x = a.x + b.x + c.x; 
            d.y = a.y + b.y + c.y; 
            d.z = a.z + b.z + c.z;
        }

        public override bool Equals(object obj)
        {
            if (obj is Pnt3D)
                return Equals((Pnt3D)obj);
            else
                return base.Equals(obj);
        }

        public bool Equals(Pnt3D other)
        {
            return this.x == other.x && this.y == other.y && this.z == other.z;
        }

        public override int GetHashCode()
        {
            return (((x.GetHashCode() << 7) + y.GetHashCode()) << 7) + z.GetHashCode();
        }
    }
}
